#include "TrianglesCreator.hpp"

vector<Polygon> TrianglesCreator::get_covered_triangles(const Polygon& original_polygon, const Point& guard)
{
	Polygon polygon = original_polygon;
	polygon.reverse_orientation();

    vector<Polygon> res;
    if (polygon.bounded_side(guard) == CGAL::ON_UNBOUNDED_SIDE)
        return res;

    vector<Segment> edges = get_edges(polygon);

    Segment first_edge = *polygon.edges_begin();
    Segment first_triangle_edge, curr_triangle_edge, last_triangle_edge;
    bool is_first_triangle_edge = true;
    for (Edge_it eit = polygon.edges_begin(); eit != polygon.edges_end(); ++eit)
    {
        Segment curr_edge = *eit;
        Segment next_edge = get_next_edge(polygon, eit);
        Point vertex = curr_edge.target();

        PointsVector intersections = get_intersections_with_edges(Ray(guard, vertex), edges, vertex);
        if (!guard_sees_vertex(polygon, intersections, guard, vertex))
            continue;

        Point last_vertex = curr_edge.source();
        Point next_vertex = next_edge.target();
        
        static const bool FIRST_EDGE = true;
        static const bool SECOND_EDGE = false;
        curr_triangle_edge = get_triangle_edge(guard, curr_edge, next_edge, intersections, SECOND_EDGE);
        if (is_first_triangle_edge)
        {
            is_first_triangle_edge = false;
            first_triangle_edge = curr_triangle_edge;
        }
        else
        {
            get_arrangement_of_triangle(res, guard, last_triangle_edge.target(), curr_triangle_edge.target());
        }

        last_triangle_edge = get_triangle_edge(guard, curr_edge, next_edge, intersections, FIRST_EDGE);
    }
	
    get_arrangement_of_triangle(res, guard, last_triangle_edge.target(), first_triangle_edge.target());
    return res;
}

vector<Segment> TrianglesCreator::get_edges(const Polygon& polygon)
{
    vector<Segment> segments;

    for(Edge_it eit = polygon.edges_begin(); eit != polygon.edges_end(); ++eit)
    {
        segments.push_back(*eit);
    }

    return segments;
}

PointsVector TrianglesCreator::get_intersections_with_edges(const Ray& ray, const vector<Segment>& edges, const Point& vertex)
{
    PointsVector intersection_points;
    for(int i = 0; i < edges.size(); ++i)
    {
        Segment edge = edges[i];
        Point p;
        bool do_intersect = intersect_segment_and_ray(edge, ray, p);
        if (do_intersect && p != vertex)
            intersection_points.push_back(p);
        
    }
    return intersection_points;
}

bool TrianglesCreator::intersect_segment_and_ray(const Segment& seg, const Ray& ray, Point& intersection_point)
{
	CGAL::cpp11::result_of<Kernel::Intersect_2(Segment, Ray)>::type 
result = intersection(seg, ray);
    //auto result = intersection(seg, ray);
    if (!result)
        return false;
	
    if(const Segment* s = boost::get<Segment>(&*result))
    {
        //TODO: support case where the intersection is a segment
        throw Exception("Not implemented!");
    } 
    else
    {
        const Point* p = boost::get<Point>(&*result);
        intersection_point = *p;
        return true;
    }

	return true;
}

bool TrianglesCreator::guard_sees_vertex(const Polygon& polygon, const PointsVector& intersections, const Point& guard, const Point& vertex)
{
    for(PointsVector::const_iterator it = intersections.begin(); it != intersections.end(); ++it)
    {
        Point p = *it;
        if (intersection_closer_to_guard(guard, vertex, p))
        {
            return false;
        }
    }

    //This check is for the case where the whole segment between the guard and the vertex is outside the polygon
    Point middle_point = guard + (vertex-guard)/2;
    if (polygon.bounded_side(middle_point) == CGAL::ON_UNBOUNDED_SIDE)
        return false;

    return true;
}

Segment TrianglesCreator::get_triangle_edge(const Point& guard, const Segment& curr_edge, const Segment& next_edge, const PointsVector& intersections, bool first_edge)
{
    Point vertex = curr_edge.target();
    Direction dir1 = curr_edge.direction();
    Direction dir2 = next_edge.direction();
    Direction segment_direction = Vector(guard, vertex).direction();
    Direction opposite_segment_direction = -segment_direction;

    if (segment_direction.counterclockwise_in_between(dir1, dir2) == opposite_segment_direction.counterclockwise_in_between(dir1, dir2))
    {
        return Segment(guard, vertex);
    }

    if (segment_direction.counterclockwise_in_between(dir1, dir2) != first_edge)
    {
        return Segment(guard, vertex);
    }

    Point closest_intersection = get_closest_intersection(guard, intersections);
    return Segment(guard, closest_intersection);
}

bool TrianglesCreator::intersection_closer_to_guard(const Point& guard, const Point& vertex, const Point& intersection_point)
{
    return ((guard - intersection_point).squared_length() < (guard-vertex).squared_length());
}

Point TrianglesCreator::get_closest_intersection(const Point& guard, const PointsVector& intersections)
{
    if (intersections.size() == 0)
        throw Exception("Intersections vector is empty");
    else if (intersections.size() == 1)
        return intersections[0];

    Point closest_point = intersections[0];
    for(int i = 1; i < intersections.size(); ++i)
    {
        if ((closest_point - guard).squared_length() > (intersections[i] - guard).squared_length())
            closest_point = intersections[i];
    }
    return closest_point;
}

Segment TrianglesCreator::get_next_edge(const Polygon& polygon, const Edge_it& eit)
{
    Edge_it next_eit = boost::next(eit, 1);
    Segment first_edge = *polygon.edges_begin();
    if (next_eit == polygon.edges_end())
        return *polygon.edges_begin();
    return *next_eit;
}

void TrianglesCreator::get_arrangement_of_triangle(vector<Polygon>& arr, const Point& point1, const Point& point2, const Point& point3)
{
	Polygon polygon;

	polygon.push_back(point2);
	polygon.push_back(point1);
	polygon.push_back(point3);

	arr.push_back(polygon);
}
